#include <iostream>
#include <fstream>
#include <string>
#include <math.h>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "Rune_fiall.h"
using namespace cv;
using namespace std;

#define USE_VIDEO

Rune::Rune()
{
    string file_name = "../rune.xml";
    FileStorage fs(file_name, FileStorage::READ);
    if(!fs.isOpened())
        printf("Cannot open armor param file, please check if the file is exist");
    else
        cout<<"succeseful load rune.xml"<<endl;
    //fs读取
    fs["enemy_color"] >> color;
    fs["Maxarea"] >> Maxarea;
    fs["Minarea"] >> Minarea;
    fs["Maxarea_l"] >> Maxarea_l;
    fs["Minarea_l"] >> Minarea_l;
    fs["img_idx"] >> img_idx;
}

void Rune::RUNE(Mat &src_1)
{
    double start = static_cast<double>(getTickCount());

    if(src_1.empty()){
        cout<<"no video"<<endl;
        return ;
    }

    gammaProcessImage(src_1,1.5,src);//提高对比度
	vector<Mat> bgr_channel;
	split(src, bgr_channel);
	if (color == 0){
        binary_color_img =bgr_channel.at(2)-bgr_channel.at(0);
        threshold(binary_color_img,binary_color_img, 50, 255, CV_THRESH_BINARY);//130
    }
	else{
        binary_color_img =bgr_channel.at(0)-bgr_channel.at(2);
        threshold(binary_color_img,binary_color_img, 150, 255, CV_THRESH_BINARY);//130
    }
    Mat binary;
    binary_color_img.copyTo(binary);
    GaussianBlur(binary,binary,Size(3,3),0,0);

    floodFill(binary_color_img,Point(0,0),Scalar(255));
    threshold(binary_color_img,binary_color_img,0,255,THRESH_BINARY_INV);
    Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
    morphologyEx(binary_color_img, binary_color_img, MORPH_OPEN, element);
    findContours(binary_color_img,contours_color, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

    vector<vector<Point> > contours_color_l;
    vector<vector<Point> > contours_color_finall;
    findContours(binary,contours_color_l, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
        
    for(int i=0;i<contours_color_l.size();i++)
    {
        double Cont = fabs(contourArea(contours_color_l[i],true));
        if(Cont < Maxarea_l && Cont > Minarea_l){
            contours_color_finall.push_back(contours_color_l[i]);
            break;
        }
    }
    if(contours_color_finall.empty()){
        return;
    }
    vector<vector<Point> >contours_ploy(contours_color.size());
    vector<RotatedRect> RotatedRect_ploy;
    vector<vector<Point> > contours_r;
    RotatedRect predict_rect;
    Point2f predict_rect_points[4],predict_rect_points_finall[4];
    float min,max;
    for(int i=0;i<contours_color.size();i++)
    {
        double Cont = fabs(contourArea(contours_color[i],true));
        approxPolyDP(contours_color[i], contours_ploy[i], 5, true);
        RotatedRect temp1 = minAreaRect(contours_ploy[i]);
        if(temp1.size.width > temp1.size.height){
            min = temp1.size.height;
            max = temp1.size.width;
        }
        else{
            max = temp1.size.height;
            min = temp1.size.width;
        }
        if(max/min >1.5 && max/min< 2.2 && Cont < Maxarea && Cont > Minarea){
            RotatedRect_ploy.push_back(temp1);
        }
        if(Cont < 70 && Cont >30){
            contours_r.push_back(contours_color[i]);
        }
    }
    Point2f pot_cen,cen_R;
    Point2f pot[4],pots[4];
    float angle;
    for (int i = 0; i< RotatedRect_ploy.size(); i++)
    {
        RotatedRect_ploy[i].points(pot);
        if(pointPolygonTest(contours_color_finall[0],RotatedRect_ploy[i].center,false) == 1)
        {
            predict_rect = RotatedRect_ploy[i];
            angle = RotatedRect_ploy[i].angle;
            //for(int j=0; j<4; j++)
            //{
                pot_cen = RotatedRect_ploy[i].center;
                //line(src, pot[j], pot[(j+1)%4], color,2);
            //}
            break;
        }
    }
//圆的方程预测
    if(contours_r.empty()){
        cen_R =his_cen_point;
    }
    else{
        cen_R = get_center_point(contours_r[0]);
        his_cen_point = cen_R;
    }
    float L_R = distance_hanshu(pot_cen,cen_R); 
    vector<RotatedRect> RotatedRect_svm;
    RotatedRect_svm = RotatedRect_ploy;
    Mat img;

    for(int i = 0;i < RotatedRect_ploy.size();i++)
    {
        RotatedRect_ploy[i].points(pot);
        if((cen_R.x <= RotatedRect_ploy[i].center.x && cen_R.y >= RotatedRect_ploy[i].center.y) ||(cen_R.x >= RotatedRect_ploy[i].center.x && cen_R.y <= RotatedRect_ploy[i].center.y))
            RotatedRect_svm[i].angle = -(90+fabs(RotatedRect_ploy[i].angle));
        else
            RotatedRect_svm[i].angle = RotatedRect_ploy[i].angle;
        
        RotatedRect_svm[i].center.x = (cen_R.x + RotatedRect_ploy[i].center.x)/2;
        RotatedRect_svm[i].center.y = (cen_R.y + RotatedRect_ploy[i].center.y)/2;
        RotatedRect_svm[i].size.width = 55.00;
        RotatedRect_svm[i].size.height = 120;//120
        RotatedRect_svm[i].points(pots);

        Mat rot_mat = cv::getRotationMatrix2D(RotatedRect_svm[i].center,RotatedRect_svm[i].angle,1); 
        auto center = RotatedRect_svm[i].center;
        warpAffine(binary, img, rot_mat, img.size(), INTER_LINEAR, BORDER_CONSTANT);  // warpAffine use 2ms
        cv::Rect target = cv::Rect(center.x - (RotatedRect_svm[i].size.width / 2), 
                                center.y - (RotatedRect_svm[i].size.height/2), 
                                RotatedRect_svm[i].size.width, RotatedRect_svm[i].size.height);

        cv::Mat armor_roi = img(target);
        /* char str[100];
        sprintf(str,"picture/armor_%d.jpg", img_idx++);
        imwrite(str,armor_roi);*/
//*********************svm*******************************
        float svl;
        svl = rune_svm(armor_roi);
        cout<<"svl:"<<svl<<endl;
        if(svl > 0 ){
            for(int j=0; j<4; j++)
                line(src, pots[j], pots[(j+1)%4], Scalar(0,0,255),2);
            for(int j=0; j<4; j++)
                line(src, pot[j], pot[(j+1)%4], Scalar(0,255,0),2);
        }
    }

    RotatedRect_ploy.clear();

    w0 = asin((pot_cen.x-cen_R.x)/L_R);
    float w1 = 0.0349*5*3;//2度对应的弧度0.0349
    if((pot_cen.x-his.x)>0 &&(pot_cen.y-his.y)<0)//判断顺时针还是逆时针
        colorwise = 1;
    else
        colorwise = 0;
    if(colorwise == 0){//顺时
        if(pot_cen.y < cen_R.y){
            predict_point.x = sin(w0+w1)*L_R+cen_R.x;
            predict_point.y = -cos(w0+w1)*L_R+cen_R.y;
        }
        else{
            predict_point.x = sin(w0-w1)*L_R+cen_R.x;
            predict_point.y = cos(w0-w1)*L_R+cen_R.y;
        }
    }
    else{//逆时针
        if(pot_cen.y < cen_R.y){
            predict_point.x = sin(w0-w1)*L_R+cen_R.x;
            predict_point.y = -cos(w0-w1)*L_R+cen_R.y;
        }
        else{
            predict_point.x = sin(w0+w1)*L_R+cen_R.x;
            predict_point.y = cos(w0+w1)*L_R+cen_R.y;
        }
    }

    his = pot_cen;

    predict_rect.center = predict_point;
    predict_rect.angle = -(fabs(angle) - w1*180/3.1415926);
    predict_rect.points(predict_rect_points_finall);
    /*for(int j=0;j<4;j++)
    {
        line(src, predict_rect_points_finall[j], predict_rect_points_finall[(j+1)%4], Scalar(255,0,0),2);
    }*/

//**********************
    imshow("img",src);
    double time = ((double)getTickCount() - start) / getTickFrequency();
    cout << time << "秒" << endl;

    contours_r.clear();
    contours_color.clear();
    contours_color_l.clear();
    contours_color_finall.clear();

    vector<Point2f> result;
    for(int i=0;i<4;i++){
        result.push_back(predict_rect_points_finall[i]);
    } 
    
    char key = (char)waitKey(0);
    if (key == 27)
        return;
}

int main()
{
    Rune rune;
    VideoCapture cap;

#ifdef USE_VIDEO 
    if(color == 0){
        cap.open("../../video/14.mp4");
        cout<<"successful load red image"<<endl;
    }
    else{
        cap.open("../../video/b1.mp4");
        cout<<"successful load blue image"<<endl;
    }
#else
    cap.open(1);
#endif

while(1)
    {
        cap >> src_1;
        rune.RUNE(src_1);
    }
    return 0;
}
